//
// Created by az on 2020/7/15.
//

#ifndef WMR_BEZIER_PLANNER_VSTTM_NAV_H
#define WMR_BEZIER_PLANNER_VSTTM_NAV_H

#include <ros/ros.h>
#include <eigen3/Eigen/Dense>
#include <vector>
#include <fstream>
#include <mutex>
#include <memory>
#include <cmath>
#include <ctime>

#include <std_msgs/Bool.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <env_edt/plan_map.h>
#include <bezier/bezier_base.h>
#include <bspline/non_uniform_bspline.h>
#include <pos_generator/oriTrans.hpp>
#include <pos_generator/loc2pos.hpp>
#include <wmr_msgs/PolynomialTrajectory.h>
#include "wmr_nav/pidcontroller.hpp"

//#include "opencv2/core.hpp"
//#include "opencv2/highgui.hpp"

#define VRESL       0.02
#define WRESL       2*D2R

using std::vector;
using std::shared_ptr;

struct EvalVSTTM {
    double v, w, qe, te;

    EvalVSTTM(double _v, double _w, double _qe, double _te) :
            v(_v), w(_w), qe(_qe), te(_te) {}
};

struct QE {
    double xe, ye, te;

    QE(double _xe, double _ye, double _te) : xe(_xe), ye(_ye), te(_te) {}
};

struct RobState{
    Eigen::Vector3d pt;
    double yaw;
    double velocity;
    double angular;

    RobState(Eigen::Vector3d& _pt, double _yaw, double _v, double _w) :
    pt(_pt), yaw(_yaw), velocity(_v), angular(_w) {}

    typedef shared_ptr<RobState> Ptr;
} ;

class VSTTM{
protected:
    struct BsplineTraj{
        NonUniformBspline pos_traj, vel_traj, acc_traj;
        double start_time;
        double duration;
    };

    /* 存储bezier轨迹的参数 */
    struct BTRAJ{
        int seg_num;
        double start_time, duration, init_yaw;
        Eigen::MatrixXd traj_coeff;
        Eigen::VectorXd seg_time;
    };

    double Vmax_, Wmax_, DVmax_, DWmax_, exe_time_, pre_time_;
    double vmin_, vmax_, wmin_, wmax_;
    double t_now_;

    bool   has_target_yaw_;
    double target_yaw_;

    double q1_, q2_, q3_, e1_, e2_;
    double v_output_, w_output_;
    double qe_sum_, te_sum_;
    double robot_radius_, dis_retraj_;

    vector<EvalVSTTM> vec_eval_;
    vector<QE> vec_QE_;
    vector<RobState::Ptr> vec_pr_;

    RobState::Ptr  p_;
    MAP_EDT::Ptr   map_edt_;
    Bernstein::Ptr bezier_;

    BTRAJ btraj_;
    BsplineTraj bspline_traj_;

    std::string path_name_;
    std::ofstream out_file_;
    std::mutex  file_lock_;
public:
    VSTTM(){
        q1_ = 2;
        q2_ = 2;
        q3_ = 0.5;

        e1_ = 1;
        e2_ = 1;

        vmin_ = vmax_ = wmin_ = wmax_ = 0.0;
        qe_sum_ = te_sum_ = 0.0;

        has_target_yaw_ = false;
        target_yaw_     = 0.0;

        Eigen::Vector3d pt = Eigen::Vector3d::Zero();
        p_ = std::make_shared<RobState>(pt, 0, 0, 0);
    }

    ~VSTTM(){
        vector<EvalVSTTM>().swap(vec_eval_);
        vector<RobState::Ptr>().swap(vec_pr_);
        vector<QE>().swap(vec_QE_);

        out_file_.close();
    }

    void init(ros::NodeHandle& nh);

    static std::string double2string(double value){
        std::ostringstream out;
        out.precision(15);
        out << value;
        return out.str();
    }

    void setEnvironment(MAP_EDT::Ptr& map_edt, Bernstein::Ptr& bezier);

    void setQ(double _q1, double _q2, double _q3){
        this->q1_ = _q1;
        this->q2_ = _q2;
        this->q3_ = _q3;
    }

    void setRobotState(double time, Eigen::Vector3d& p_3d, double& yaw, double& v, double& w);

    void setBTraj(int seg_num, double stime, Eigen::MatrixXd& coeff, Eigen::VectorXd& seg_time);

    void setBspline(vector<NonUniformBspline> traj, double start_t);

    void setStartTime(double start_t);

    bool run(double& velocity, double& angular);

    void recordError();

    //angle control
    void anglePIDinit(double init_yaw);

    void anglePIDCtrl(bool& _finish, double& _vel, double& _anl);

    double getRobMaxVel() {return Vmax_;}
    double getRobMaxAvl() {return Wmax_;}

    typedef shared_ptr<VSTTM> Ptr;
protected:
    void calcDynamicWindow(double& vmin, double& vmax, double& wmin, double& wmax);
    void genTrajPoint(RobState& p_eval, const double& vt, const double& wt, const double& t);

    void evaluation();
    void normalize();
    void finalEvaluate();
    bool getPr();
    bool calcQE(const double& vt, const double& wt, double& qe, double& dis, vector<RobState>& traj);
    double CalcTermC(RobState& pt);
};

#endif //WMR_BEZIER_PLANNER_VSTTM_NAV_H
